#!/usr/bin/env python3
# -*- coding: utf-8 -*-
from scipy.spatial.distance import cdist
from collections import Counter
import math
import numpy as np
import random
def degree_to_rdian(degree):
    return degree * math.pi* 0.005556

def radian_to_degree(radian):
    return radian * 180 / math.pi

def get_bottem_left_point_of_rectangle_origin(center_x, center_y, height, width, angle):
    
    angle = angle * math.pi / 180

    #top right:
    top_right_x = center_x + ((width / 2) * math.cos(angle)) - ((height / 2) * math.sin(angle))
    top_right_y = center_y + ((width / 2) * math.sin(angle)) + ((height / 2) * math.cos(angle))
       
    #top left:
    top_left_x = center_x - ((width / 2) * math.cos(angle)) - ((height / 2) * math.sin(angle))
    top_left_y = center_y - ((width / 2) * math.sin(angle)) + ((height / 2) * math.cos(angle))
        
    #bottom left:
    bot_left_x = center_x + ((width / 2) * math.sin(angle)) - ((height / 2) * math.cos(angle))
    bot_left_y = center_y - ((width / 2) * math.cos(angle)) - ((height / 2) * math.sin(angle))
         
    #bottom right:
    bot_right_x = center_x + ((width / 2) * math.cos(angle)) + ((height / 2) * math.sin(angle))
    bot_right_y = center_y + ((width / 2) * math.sin(angle)) - ((height / 2) * math.cos(angle))

    #bottom center:
    bot_center_x = 0.5 * (bot_right_x + bot_left_x)
    bot_center_y = 0.5 * (bot_right_y + bot_left_y)

    return bot_left_x, bot_left_y

def get_bottem_left_point_of_rectangle(center_x, center_y, height, width, angle):
            
    angle = angle * math.pi / 180

    #top right:
    top_right_x = center_x + ((width / 2) * math.cos(angle)) - ((height / 2) * math.sin(angle))
    top_right_y = center_y + ((width / 2) * math.sin(angle)) + ((height / 2) * math.cos(angle))
       
    #top left:
    top_left_x = center_x - ((width / 2) * math.cos(angle)) - ((height / 2) * math.sin(angle))
    top_left_y = center_y - ((width / 2) * math.sin(angle)) + ((height / 2) * math.cos(angle))
        
    #bottom left:
    bot_left_x = center_x - ((width / 2) * math.cos(angle)) + ((height / 2) * math.sin(angle))
    bot_left_y = center_y - ((width / 2) * math.sin(angle)) - ((height / 2) * math.cos(angle))
         
    #bottom right:
    bot_right_x = center_x + ((width / 2) * math.cos(angle)) + ((height / 2) * math.sin(angle))
    bot_right_y = center_y + ((width / 2) * math.sin(angle)) - ((height / 2) * math.cos(angle))

    #bottom center:
    bot_center_x = 0.5 * (bot_right_x + bot_left_x)
    bot_center_y = 0.5 * (bot_right_y + bot_left_y)
    
    angle = radian_to_degree(angle)
    
    if math.sin(angle) >= 0 and math.cos(angle) >= 0:
        return bot_left_x, bot_left_y
    if math.sin(angle) >= 0 and math.cos(angle) <= 0:
        return bot_left_x, bot_left_y   
    if math.sin(angle) <= 0 and math.cos(angle) >= 0:
        return bot_right_x, bot_right_y
    if math.sin(angle) <= 0 and math.cos(angle) <= 0:
        return bot_right_x, bot_right_y    
       


#Draw the heading arrow of the agent. x,y are the position of this arrpw and dx,dy are dir vectors
def calculate_arrow_base(yaw_theta, rectangle_x, rectangle_y, rectangle_length, rectangle_width):
    theta = yaw_theta + 90
    if theta >= 360:
        theta = theta - 360
    if theta <= -360:
        theta = theta + 360
    if theta >= 180:
        theta = theta - 360
    if theta <= -180:
        theta = theta + 360        

    theta = theta * math.pi / 180
    yaw_theta = yaw_theta * math.pi / 180
    x = rectangle_x + 0.5 * rectangle_width * math.cos(theta)
    y = rectangle_y + 0.5 * rectangle_width * math.sin(theta)
    dx = (rectangle_length + 1) * math.cos(yaw_theta)
    dy = (rectangle_length + 1) * math.sin(yaw_theta)  
    return x, y,dx,dy

# def find_preceeding_following_in_lane(scenario, time_index, agent_id):
#     for agent in scenario.agents_list:
#         if agent.agent_id == agent_id:
#             current_agent = agent
#             break
#     else:
#         return None, None
#     current_agent_x = current_agent.states_array[time_index][3]
#     current_agent_y = current_agent.states_array[time_index][4]
#     current_agent_width = current_agent.states_array[time_index][7]
#     current_agent_length = current_agent.states_array[time_index][6]
#     currnet_lane_id = on_which_lane(scenario,current_agent_x,current_agent_y,current_agent_length,current_agent_width)
    
#     min_distance = float("inf")
#     preceding_agent = None
#     following_agent = None
#     for agent in scenario.agents_list:
#         if agent.agent_id != agent_id and agent.agent_type == 1:
#             agent_x = agent.states_array[time_index][3]
#             agent_y = agent.states_array[time_index][4]
#             agent_yaw = agent.states_array[time_index][9]
            
#             dx = agent_x - current_agent_x
#             dy = agent_y - current_agent_y
#             distance = np.sqrt(dx**2 + dy**2)
#             angle = np.arctan2(dy, dx) - agent_yaw
            
#             if abs(angle) > np.pi / 2:
#                 continue
            
#             if abs(agent_yaw-current_agent.states_array[time_index][9]) > np.pi /4 :
#                 continue 
            
#             if distance < min_distance:
#                 min_distance = distance
#                 dx_perp = np.sin(angle) * distance
#                 dy_perp = np.cos(angle) * distance
#                 following_agent_x = agent_x - dx_perp
#                 following_agent_y = agent_y - dy_perp
#                 preceding_agent_x = agent_x + dx_perp
#                 preceding_agent_y = agent_y + dy_perp
#                 if is_on_same_lane(scenario, following_agent_x, following_agent_y, current_agent_length, current_agent_width,currnet_lane_id):
#                     following_agent = agent
#                 if is_on_same_lane(scenario, preceding_agent_x, preceding_agent_y, current_agent_length, current_agent_width,currnet_lane_id):
#                     preceding_agent = agent
    
#     return preceding_agent, following_agent

# def get_distance_to_lane(x, y, lane):
#     # calculate the minimum distance between the vehicle and the lane
#     min_distance = float('inf')
#     for i in range(len(lane.points_list) - 1):
#         x1, y1 = lane.points_list[i]
#         x2, y2 = lane.points_list[i+1]
#         # print((x1,y1,x2,y2))
#         distance = point_to_line_distance(x, y, x1, y1, x2, y2)
#         if distance < min_distance:
#             min_distance = distance
#     return min_distance

# def get_lane_distance_and_index(x, y, scenario):
#     min_distance = float('inf')
#     min_distance_index = None
#     for i, lane in enumerate(scenario.center_lane_list):
#         distance = get_distance_to_lane(x, y, lane)
#         if distance < min_distance:
#             min_distance = distance
#             min_distance_index = i
#     return min_distance, min_distance_index

# def on_which_lane(scenario, x, y, length, width):
#     min_distance, min_distance_index = get_lane_distance_and_index(x, y, scenario)
#     if min_distance > max(length, width) / 2:
#         return None
#     else:
#         print((min_distance, min_distance_index))
#         return scenario.center_lane_list[min_distance_index].lane_id


# def is_on_same_lane(scenario, x, y, length,width, lane_id):
#     lane = scenario.center_lane_dict[lane_id]
#     for i in range(len(lane.points_list) - 1):
#         x1, y1 = lane.points_list[i]
#         x2, y2 = lane.points_list[i+1]
#         distance = point_to_line_distance(x, y, x1, y1, x2, y2)
#         if distance <= max(width, length)/2:
#             return True
#     return False

# def point_to_line_distance(x, y, x1, y1, x2, y2):
#     px = x2-x1
#     py = y2-y1
#     temp = px*px + py*py
#     # if(float(temp) == 0):
#     #     print("zhedouxing")
#     if(temp==0):
#         return 1000
#     u =  ((x - x1) * px + (y - y1) * py) / float(temp)
#     if u > 1:
#         u = 1
#     elif u < 0:
#         u = 0
#     x = x1 + u * px
#     y = y1 + u * py
#     dx = x - x1
#     dy = y - y1
#     return np.sqrt(dx*dx + dy*dy)

def lane_id_to_index(lane_list,ilane_id):
    i = 0
    for lane in lane_list:
        if lane == ilane_id:
            return i
        else: 
            i += 1

def Id_to_Index(agents_list, agent_id):
    i = 0
    for agent in agents_list:
        if agent.agent_id == agent_id:
            return i
        else:
            i+=1
            
def find_front_back_vehicle(scenario, time_index, agent_id):
    for agent in scenario.agents_list:
        if agent.agent_id == agent_id:
            current_agent = agent
            break
    else:
        return None, None
    current_agent_x = current_agent.states_array[time_index][3]
    current_agent_y = current_agent.states_array[time_index][4]
    current_agent_width = current_agent.states_array[time_index][7]
    current_agent_length = current_agent.states_array[time_index][6]

    current_agent_heading = current_agent.states_array[time_index][9]
    front_distance = float('inf')
    back_distance = float('inf')
    front_agent_id = None
    back_agent_id = None
    for agent in scenario.agents_list:
        if agent.agent_id != agent_id and agent.agent_type == 1:
            agent_x = agent.states_array[time_index][3]
            agent_y = agent.states_array[time_index][4]
            agent_heading = agent.states_array[time_index][9]
            
            x_rel = math.cos(current_agent_heading) * (agent_x - current_agent_x) + math.sin(current_agent_heading) * (agent_y - current_agent_y)
            y_rel = -math.sin(current_agent_heading) * (agent_x - current_agent_x) + math.cos(current_agent_heading) * (agent_y - current_agent_y)
            theta = math.atan2(y_rel, x_rel) - current_agent_heading
            
            if x_rel < current_agent_length * 5 and abs(y_rel) <= current_agent_width * 1.4 / 2:
                if x_rel >= 0 and (front_agent_id is None or x_rel > scenario.agents_list[Id_to_Index(scenario.agents_list,front_agent_id)].states_array[1][3]-current_agent_x):
                    heading_diff = abs(agent_heading - current_agent_heading)
                    heading_diff = min(heading_diff, 2 * math.pi - heading_diff)
                    if heading_diff < math.pi / 4:
                        front_agent_id = agent.agent_id
                elif x_rel < 0 and (back_agent_id is None or x_rel < scenario.agents_list[Id_to_Index(scenario.agents_list,back_agent_id)].states_array[1][3]-current_agent_x):
                    heading_diff = abs(agent_heading - current_agent_heading)
                    heading_diff = min(heading_diff, 2 * math.pi - heading_diff)
                    if heading_diff < math.pi / 4:
                        back_agent_id = agent.agent_id
 
    # if front_agent_id is None:
    #     print("No acceptable front vehicle!")
    
    # if back_agent_id is None:
    #     print("No acceptable back vehicle!")
        
    return front_agent_id, back_agent_id


def initial_lane_of_track(lane_dist, lane, track_x_ini,track_y_ini):
    current_lane_id = 0
    distance = 100000
    for key in lane_dist:
        for i in range(0,min(len(lane_dist[key][0]),len(lane_dist[key][1]))):
            tmp = math.sqrt((lane_dist[key][0][i]-track_x_ini)**2+(lane_dist[key][1][i]-track_y_ini)**2)
            if tmp < distance:
                distance = tmp
                current_lane_id = key
    left_neighbors_ids = lane[current_lane_id].left_neighbors_id_dict[current_lane_id]
    right_neighbors_ids = lane[current_lane_id].right_neighbors_id_dict[current_lane_id]
    entry_lane_ids = lane[current_lane_id].entry_lane_id_dict[current_lane_id]
    exit_lane_ids = lane[current_lane_id].exit_lane_id_dict[current_lane_id]   
    
    return current_lane_id, left_neighbors_ids, right_neighbors_ids, entry_lane_ids, exit_lane_ids 

def interp_center_position(position_arr):
    start_zeros = np.argmax(position_arr != 0)
    end_zeros = np.argmax(np.flip(position_arr) != 0)

    indices = np.arange(len(position_arr))
    nonzero_indices = indices[position_arr != 0]
    nonzero_values = position_arr[position_arr!= 0]
    if len(nonzero_indices) != 0 and len(nonzero_values)!= 0:
        interp_values = np.interp(indices, nonzero_indices, nonzero_values)
    
        if start_zeros >= 3:
            interp_values[:start_zeros] = None
        if end_zeros >= 3:
            interp_values[-end_zeros:] = None    
        return interp_values    
    else:
        return position_arr

def calculate_slope(x1, y1, x2, y2):
    slope = np.divide((y2 - y1), (x2 - x1))
    return slope

def angle_to_vector(angle):
    # 将弧度转换为向量，朝向角为angle
    return np.array([np.cos(angle), np.sin(angle)])

def is_lane_straight(lanex,laney,threshold):
    # slopes = np.diff(laney) / np.diff(lanex)
    # max_slope_diff = np.max(slopes) - np.min(slopes)
   
    lane_px1, lane_px2 = lanex[0], lanex[-1]
    lane_py1, lane_py2 = laney[0], laney[-1]
    
    px = lanex[int(len(lanex)/2)]
    py = laney[int(len(laney)/2)]
    
    line_point1 = np.array([lane_px1,lane_py1])
    line_point2 = np.array([lane_px2,lane_py2])
    vec1 = np.array([lane_px1-px,lane_py1-py])
    vec2 = np.array([lane_px2-px,lane_py2-py])
    distance = np.abs(np.cross(vec1, vec2)) / np.linalg.norm(line_point1 - line_point2)
    if distance < threshold:
        return True
    else:
        return False
    
    
def judge_front_or_back(x1,x2,y1,y2,h1,h2):
    dot_product = (x2 - x1) * np.cos(h1) + (y2 - y1) * np.sin(h1)
    norm_vector = np.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
    norm_h1_vector = 1.0  # 因为朝向角的向量模长为1
    
    # 计算夹角的余弦值
    cos_theta = dot_product / (norm_vector * norm_h1_vector)
    
    # 计算夹角的弧度值
    angle_rad = np.arccos(cos_theta)
    angle1 = np.degrees(h1)
    angle2 = np.degrees(h2)
    ch = abs(angle1 - angle2)
    
    if angle_rad >= np.pi/2:
            return True
    else:
            return False
        
def wrap_to_pi(radian_angle):
    # 将弧度制角度转换到区间[-π, π]
    wrapped_angle = (radian_angle + np.pi) % (2 * np.pi) - np.pi
    # if wrapped_angle < 0:
    #     wrapped_angle += 2 * np.pi
    return wrapped_angle

def euclidean_distance(point1, point2):
    x1, y1 = point1
    x2, y2 = point2
    return math.sqrt((x2 - x1)**2 + (y2 - y1)**2)

def binary_search_nearest(xlist,ylist, target_point):
    left, right = 0, min(len(xlist)-1,len(ylist)-1)
    nearest_index = -1
    flag = False
    min_distance = 1000000
    for i in range(right):
        point_cal = (xlist[i],ylist[i])
        distance = euclidean_distance(point_cal, target_point)
        if distance < min_distance:
            min_distance = distance
            nearest_index = i
    return nearest_index,min_distance

def find_nearest_point_index(x, y, xlist,ylist):
    target_point = (x, y)
    nearest_index,min_distance = binary_search_nearest(xlist,ylist, target_point)
    return nearest_index,min_distance

def get_Manhattan_distance(position, lane_x, lane_y, interval = 8):
    px, py = position[0], position[1]
    snumber = min(len(lane_x), len(lane_y))
    
    selected_indices = list(range(0, snumber, interval))  # 使用等间隔选取的索引
    
    distance_list = []
    for i in selected_indices:
        lx = lane_x[i]
        ly = lane_y[i]
        dis = abs(px - lx) + abs(py - ly)
        #dis = (px - lx) ** 2 + (py - ly) ** 2
        distance_list.append(dis)
    
    if len(distance_list) != 0:
        distance_mean = sum(distance_list) / len(distance_list)
        return distance_mean
    else:
        return 100000


def most_common_element_and_count(lst):
    counter = Counter(lst)
    most_common = counter.most_common(1)  # 获取出现次数最多的元素及其数量
    if most_common:
        return most_common[0][0], most_common[0][1]  # 返回数量最多的元素和数量
    else:
        return None, 0  # 如果列表为空，则返回None和0
    
def find_min_distance_and_ID(x1,x2):
    x1 = np.array(x1)
    distance_set = cdist (x1,x2,metric = 'euclidean')
    max_index = np.argmax(distance_set)
    max_dis = distance_set[max_index]
    return max_index,max_dis

def list_to_tuple(ll):
    px = ll[0]
    py = ll[1]
    out = []
    for x,y in zip(px,py):
        out.append((x,y))
    return out